//
// Created by zzm on 2021/12/3.
//

#ifndef DYNAMIC_CONNECT_UNIONSYS_CORE_H
#define DYNAMIC_CONNECT_UNIONSYS_CORE_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/hdreg.h>

#include <unistd.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <ifaddrs.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <linux/if.h>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <mavros_msgs/SetMavFrame.h>
#include <std_msgs/Int8.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <vector>
#include <algorithm>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32MultiArray.h>

// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <std_msgs/String.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/RCOut.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamSet.h>

class RegisterMSN{
public:

    RegisterMSN();
    ~RegisterMSN();

    //get the dds pub max size
    int get_dds_pub_size();

    //limit the square vx vy
    geometry_msgs::Point limit_velocity(float vx, float vy,float maximum);

    //transform quaternion to eular angle
    geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat);

    //add the offset between camera and vehicle ,delta_x means Position of camera on plane coord
    geometry_msgs::Point rotate_Rcg_Ppc(geometry_msgs::Vector3 angle_cg, geometry_msgs::Vector3 angle_cp,float delta_x, float delta_y, float delta_z);

    
    geometry_msgs::Point rotation_hehe(geometry_msgs::Vector3 angle, float delta_x, float delta_y, float delta_z);

    //calculate the /camera/pose topic outputs
    geometry_msgs::PoseStamped calculate_cam_pos(nav_msgs::Odometry current_info, Eigen::Matrix3d Rpitch, Eigen::Matrix3d Rroll);

    geometry_msgs::PoseStamped calculate_cam_pos(nav_msgs::Odometry current_info, float roll, float pitch , float yaw);

    //calculate the position offset between apriltag and vehicle
    geometry_msgs::Vector3 calc_P_offset(Eigen::Matrix3d R_cam_tag, geometry_msgs::Point P_cam_tag, geometry_msgs::Vector3 P_plane_cam, 
                                    geometry_msgs::Point P_plane_home, Eigen::Matrix3d R_tag_world, geometry_msgs::Vector3 P_tag_world);
    geometry_msgs::Quaternion calc_Q_offset(geometry_msgs::Quaternion Q_cam_tag, geometry_msgs::Quaternion Q_tag_world,
                                             geometry_msgs::Quaternion Q_plane_cam, geometry_msgs::Quaternion Q_plane_home);

    //quaternion mutiply:q2Xq1
    geometry_msgs::Quaternion quatMultiply(geometry_msgs::Quaternion q2, geometry_msgs::Quaternion q1);

    //change quaternion to rotation matrix
    Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w);

    //multiply rotation matrix with [x y z ]
    geometry_msgs::Vector3 R_multi_P(Eigen::Matrix3d R, double x, double y, double z);

    //multiply rotaion matrix R1XR2
    Eigen::Matrix3d R_multi_R(Eigen::Matrix3d R1, Eigen::Matrix3d R2);

    //add two int number to a string 
    std::string serial_str(int drone_id, int mission_state);

    //deserial string by "/"
    std::vector<std::string> deserial_str(std::string str);

    //fisheye camera model function
    double fisheye_model_dist2pitch(double dist);
    double fisheye_model_focus(double pitch_deg);



    //print register variable
    void print_var_reg();

    //register laptop with key password
    void print_var_reg(int key);

    //register nx by machine serial number
    void detect_serial_number();

};

#endif //DYNAMIC_CONNECT_UNIONSYS_CORE_H
